// => Matrix

// Includes

#include "matrix/matrix.h"

#include <math.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "matrix/matrix_ext.h"

// Functions

/// @func: InitMatrixFromArray
/// >> init a matrix from a double array
/// @param: <row> the row of the matrix [ size_t ]
/// @param: <col> the column of the matrix [ size_t ]
/// @param: <arr> the array [ double * ]
/// @param: <ort> the orientation of matrix [ Orientation ]
/// @return: the matrix from array [ Matrix * ]
/// @descript:
///   * exit when `arr` is a null pointer
///   * if `ort` is 0, then matrix will be filled by row
///   * if `ort` is 1, then matrix will be filled by column
Matrix *InitMatrixFromArray(size_t row, size_t col, double *arr,
                            Orientation ort) {
  if (NULL == arr) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  }

  Matrix *mat = Zeros(row, col);

  for (int i = 1; i <= mat->size[0]; ++i) {
    for (int j = 1; j <= mat->size[1]; ++j) {
      if (ort == ROW) {
        SetMatrixValue(mat, i, j, arr[(i - 1) * mat->size[1] + (j - 1)]);
      } else if (ort == COL) {
        SetMatrixValue(mat, i, j, arr[(j - 1) * mat->size[0] + (i - 1)]);
      }
    }
  }

  return mat;
}

/// @func: GetMatrixValue
/// >> get the value at a specific position in the matrix
/// @param: <mat> the matrix [ const Matrix * ]
/// @param: <row> the row position [ size_t ]
/// @param: <col> the column position [ size_t ]
/// @return: the value at the specific position [ double ]
/// @descript:
///   * `row` and `col` start from 1
///   * exit when `mat` is a null pointer
///   * exit when the position is out of boundary
double GetMatrixValue(const Matrix *mat, size_t row, size_t col) {
  if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (row < 1 || row > mat->size[0] || col < 1 || col > mat->size[1]) {
    fprintf(stderr, "Out of Boundary Error!");
    exit(EXIT_FAILURE);
  }

  return mat->data[(row - 1) * mat->size[1] + (col - 1)];
}

/// @func: SetMatrixValue
/// >> set the value at a specific position in the matrix
/// @param: <mat> the matrix [ Matrix * ]
/// @param: <row> the row position [ size_t ]
/// @param: <col> the column position [ size_t ]
/// @param: <val> the value [ double ]
/// @descript:
///   * `row` and `col` start from 1
///   * exit when `mat` is a null pointer
///   * exit when the position is out of boundary
void SetMatrixValue(Matrix *mat, size_t row, size_t col, double val) {
  if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (row < 1 || row > mat->size[0] || col < 1 || col > mat->size[1]) {
    fprintf(stderr, "Out of Boundary Error!");
    exit(EXIT_FAILURE);
  }

  mat->data[(row - 1) * mat->size[1] + (col - 1)] = val;
}

/// @func: Zeros
/// >> get a zeros matrix with specific size
/// @param: <row> the row of the matrix [ size_t ]
/// @param: <col> the column of the matrix [ size_t ]
/// @return: a matrix fill with zeros [ Matrix * ]
Matrix *Zeros(size_t row, size_t col) { return InitMatrix(row, col, 0); }

/// @func: InitMatrixDiag
/// >> init a diagonal matrix
/// @param: <arr> the value of diagnal element [ double * ]
/// @param: <len> the actual size of the matrix [ size_t ]
/// @return: the diagonal matrix [ Matrix * ]
/// @descript: exit when `arr` is a null pointer
Matrix *InitMatrixDiag(double *arr, size_t len) {
  if (NULL == arr) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (len < 1) {
    fprintf(stderr, "Wrong Size to Matrix");
    exit(EXIT_FAILURE);
  }

  Matrix *mat = Zeros(len, len);
  for (int i = 1; i <= len; ++i) {
    SetMatrixValue(mat, i, i, arr[i]);
  }

  return mat;
}

/// @func: Eye
/// >> init an identity matrix
/// @param: <row> the row of the matrix [ size_t ]
/// @param: <col> the column of the matrix [ size_t ]
/// @return: an identity matrix [ Matrix * ]
Matrix *Eye(size_t row, size_t col) {
  if (row < 1 || col < 1) {
    fprintf(stderr, "Wrong Size to Matrix");
    exit(EXIT_FAILURE);
  }

  Matrix *mat = Zeros(row, col);
  size_t len = row > col ? col : row;
  for (int i = 1; i <= len; ++i) {
    SetMatrixValue(mat, i, i, 1);
  }

  return mat;
}

/// @func: InitRandIntMatrix
/// >> init a matrix with all random integers
/// @param: <row> the row of the matrix [ size_t ]
/// @param: <col> the column of the matrix [ size_t ]
/// @param: <lb> the lower boundary [ int ]
/// @param: <ub> the upper boundary [ int ]
/// @param: <seed> the random seed for srand [ unsigned int ]
/// @return: the matrix [ Matrix * ]
/// @descript:
///   * range is [`lb`, `ub`], include endpoints
///   * exit when `ub` is smaller than `lb`
Matrix *InitRandIntMatrix(size_t row, size_t col, int lb, int ub,
                          unsigned int seed) {
  if (ub < lb) {
    fprintf(stderr, "Wrong Boundary Error!");
    exit(EXIT_FAILURE);
  }

  srand(seed);
  Matrix *mat = Zeros(row, col);
  for (size_t i = 1; i <= mat->size[0]; ++i) {
    for (size_t j = 1; j <= mat->size[1]; ++j) {
      SetMatrixValue(mat, i, j, (double)(rand() % (ub - lb + 1) + lb));
    }
  }

  return mat;
}

/// @func: InitRandRealMatrix
/// >> init a matrix with all random integers
/// @param: <row> the row of the matrix [ size_t ]
/// @param: <col> the column of the matrix [ size_t ]
/// @param: <lb> the lower boundary [ double ]
/// @param: <ub> the upper boundary [ double ]
/// @param: <seed> the random seed for srand [ unsigned int ]
/// @return: the matrix [ Matrix * ]
/// @descript:
///   * range is [`lb`, `ub`], include endpoints
///   * exit when `ub` is smaller than `lb`
Matrix *InitRandRealMatrix(size_t row, size_t col, double lb, double ub,
                           unsigned int seed) {
  if (ub < lb) {
    fprintf(stderr, "Wrong Boundary Error!");
    exit(EXIT_FAILURE);
  }

  srand(seed);
  Matrix *mat = Zeros(row, col);
  for (size_t i = 1; i <= mat->size[0]; ++i) {
    for (size_t j = 1; j <= mat->size[1]; ++j) {
      SetMatrixValue(mat, i, j,
                     ((double)rand() / (double)(RAND_MAX)) * (ub - lb) + lb);
    }
  }

  return mat;
}

/// @func: GetMatrixDiag
/// >> get all the diagonal element of the matrix
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: a column vector [ Matrix * ]
/// @descript: exit when `mat` is a null pointer
Matrix *GetMatrixDiag(const Matrix *mat) {
  if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  }
  size_t len = mat->size[0] > mat->size[1] ? mat->size[1] : mat->size[0];
  Matrix *diag = Zeros(len, 1);

  for (int i = 1; i <= len; ++i) {
    SetMatrixValue(diag, i, 1, GetMatrixValue(mat, i, i));
  }

  return diag;
}

/// @func: GetMatrixRank
/// >> get matrix rank
/// @param: <mat> the matrix [const Matrix * ]
/// @return: the rank of the matrix [ size_t ]
/// @descript: exit when `mat` is a null pointer
size_t GetMatrixRank(const Matrix *mat) {
#define min(x, y) ((x) > (y) ? (y) : (x))
#define abs(x) ((x) < 0 ? -(x) : (x))
  const double tol = 1e-5;
  LURes *lures = MatrixLUDecomposition(mat);
  size_t len = min(lures->matrixU->size[0], lures->matrixU->size[1]);
  size_t rank = 0;

  for (size_t i = 1; i <= len; ++i) {
    if (abs(GetMatrixValue(mat, i, i)) > tol) {
      rank++;
    }
  }

  DestroyLURes(lures);
#undef abs
#undef min

  return rank;
}

/// @func: TransposeMatrix
/// >> transpose a matrix
/// @param: <mat> the matrix [ const Matrix * ]
/// @param: the transposed matrix [ Matrix * ]
Matrix *TransposeMatrix(const Matrix *mat) {
  Matrix *transM = Zeros(mat->size[1], mat->size[0]);

  for (size_t i = 1; i <= transM->size[0]; ++i) {
    for (size_t j = 1; j <= transM->size[1]; ++j) {
      SetMatrixValue(transM, i, j, GetMatrixValue(mat, j, i));
    }
  }

  return transM;
}

/// @func: PrintMatrix
/// >> print matrix
/// @param: <mat> the matrix [ const Matrix * ]
/// @descript: exit when `mat` is a null pointer
void PrintMatrix(const Matrix *mat) { PrintMatrixP(mat, 3); }

/// @func: DestroyMatrix
/// >> deconstruct a matrix
/// @param: <mat> the matrix [ Matrix * ]
void DestroyMatrix(Matrix *mat) {
  if (NULL == mat) {
    return;
  }

  free(mat->data);
  free(mat);

  mat = NULL;

  return;
}

/// @func: MatrixAdd
/// >> add up two matrices
/// @param: <m1> the first matrix [ const Matrix * ]
/// @param: <m2> the second matrix [ const Matrix * ]
/// @return: the sum of two matrices [ Matrix * ]
/// @descript:
///   * exit when `m1` or `m2` is a null pointer
///   * exit when the size of `m1` and `m2` are not matched
Matrix *MatrixAdd(const Matrix *m1, const Matrix *m2) {
  if (NULL == m1 || NULL == m2) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (m1->size[0] != m2->size[0] || m1->size[1] != m2->size[1]) {
    fprintf(stderr, "Unmatched Size Error!");
  }

  Matrix *sum = Zeros(m1->size[0], m2->size[1]);

  for (int i = 1; i <= m2->size[0]; ++i) {
    for (int j = 1; j <= m1->size[1]; ++j) {
      SetMatrixValue(sum, i, j,
                     GetMatrixValue(m1, i, j) + GetMatrixValue(m2, i, j));
    }
  }

  return sum;
}

/// @func: ScalarMatrixAdd
/// >> add up a matrix and a real number
/// @param: <val> the real number [ double ]
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: the sum [ Matrix * ]
/// @descript: exit when `mat` is a null pointer
Matrix *ScalarMatrixAdd(double val, const Matrix *mat) {
  if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  }

  Matrix *sum = Zeros(mat->size[0], mat->size[1]);

  for (int i = 1; i <= mat->size[0]; ++i) {
    for (int j = 1; j <= mat->size[1]; ++j) {
      SetMatrixValue(sum, i, j, GetMatrixValue(mat, i, j) + val);
    }
  }

  return sum;
}

/// @func: ScalarMatrixMul
/// >> multiply a matrix and a real number
/// @param: <val> the real number [ double ]
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: the product [ Matrix * ]
/// @descript: exit when `mat` is a null pointer
Matrix *ScalarMatrixMul(double val, const Matrix *mat) {
  if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  }

  Matrix *prod = Zeros(mat->size[0], mat->size[1]);

  for (int i = 1; i <= mat->size[0]; ++i) {
    for (int j = 1; j <= mat->size[1]; ++j) {
      SetMatrixValue(prod, i, j, GetMatrixValue(mat, i, j) * val);
    }
  }

  return prod;
}

/// @func: ScalarMatrixDiv
/// >> divide a matrix and a real number
/// @param: <mat> the matrix [ const Matrix * ]
/// @param: <val> the real number [ double ]
/// @return: the product [ Matrix * ]
/// @descript:
///   * exit when `mat` is a null pointer
///   * exit when `val` is zero
Matrix *ScalarMatrixDiv(const Matrix *mat, double val) {
#define abs(x) ((x) < 0 ? -(x) : (x))
  const double tol = 1e-5;
  if (abs(val) < tol) {
    fprintf(stderr, "Divide By Zero Error!");
    exit(EXIT_FAILURE);
  } else if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  }
#undef abs

  return ScalarMatrixMul(1 / val, mat);
}

/// @func: MatrixMul
/// >> multiply two matrices
/// @param: <m1> the first matrix [ const Matrix * ]
/// @param: <m2> the second matrix [ const Matrix * ]
/// @return: the product of two matrices [ Matrix * ]
/// @descript:
///   * exit when `m1` or `m2` is a null pointer
///   * exit when the size of `m1` and `m2` are not matched
Matrix *MatrixMul(const Matrix *m1, const Matrix *m2) {
  if (NULL == m1 || NULL == m2) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (m1->size[1] != m2->size[0]) {
    fprintf(stderr, "Unmatched Size Error!");
  }

  Matrix *prod = Zeros(m1->size[0], m2->size[1]);

  for (int i = 1; i <= m1->size[0]; ++i) {
    for (int j = 1; j <= m2->size[1]; ++j) {
      double sum = 0.0;
      for (int k = 1; k <= m1->size[1]; ++k) {
        sum += GetMatrixValue(m1, i, k) * GetMatrixValue(m2, k, j);
      }
      SetMatrixValue(prod, i, j, sum);
    }
  }

  return prod;
}

/// @func: InnerProduct
/// >> inner product for matrix
/// @param: <m1> the first matrix [ const Matrix * ]
/// @param: <m2> the second matrix [ const Matrix * ]
/// @return: the product of two matrices [ Matrix * ]
/// @descript:
///   * exit when `m1` or `m2` is a null pointer
///   * exit when the size of `m1` and `m2` are not matched
Matrix *InnerProduct(const Matrix *m1, const Matrix *m2) {
  // (x, y) = x^T . y
  Matrix *trans = TransposeMatrix(m1);
  Matrix *res = MatrixMul(trans, m2);
  DestroyMatrix(trans);
  return res;
}

/// @func: VectorNorm
/// >> get vector norm
/// @param: <vec> the vector [ const Matrix * ]
/// @param: <t> the type of norm [ const char * ]
/// @return: the norm of vector [ double ]
/// @descript:
///   * exit when `vec` is a null pointer
///   * `t` can only be `euc`, `one` or `inf`
double VectorNorm(const Matrix *vec, const char *t) {
  if (NULL == vec) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (1 == vec->size[0] || 1 == vec->size[1]) {
    fprintf(stderr, "Not a Vector Error!");
    exit(EXIT_FAILURE);
  }

  size_t len = 1 == vec->size[0] ? vec->size[1] : vec->size[0];
  if (strcmp(t, "euc") == 0) {
    double sum = 0.0;
    for (size_t i = 0; i < len; ++i) {
      sum += vec->data[i] * vec->data[i];
    }
    return sqrt(sum);
  } else if (strcmp(t, "one") == 0) {
    double sum = 0.0;
    for (size_t i = 0; i < len; ++i) {
      sum += fabs(vec->data[i]);
    }
    return sum;
  } else if (strcmp(t, "inf") == 0) {
    double norm = fabs(vec->data[0]);
    for (size_t i = 1; i < len; ++i) {
      if (fabs(vec->data[i]) > norm) {
        norm = fabs(vec->data[i]);
      }
      return norm;
    }
  }
  fprintf(stderr, "Wrong Type of Norm Error!");
  exit(EXIT_FAILURE);
}

/// @func: MatrixTrace
/// >> get the trace of matrix
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: the trace  [ double ]
/// @descript:
///   * exit when `mat` is a null pointer
///   * exit when `mat` is not a square matrix
double MatrixTrace(Matrix *mat) {
  if (mat->size[0] != mat->size[1]) {
    fprintf(stderr, "Not a Square Matrix Error!");
    exit(EXIT_FAILURE);
  }

  LURes *lures = MatrixLUDecomposition(mat);
  double trace = 0.0;

  for (size_t i = 1; i < lures->matrixU->size[0]; ++i) {
    trace += GetMatrixValue(lures->matrixU, i, i);
  }

  DestroyLURes(lures);

  return trace;
}

/// @func: IsEqualMatrix
/// >> test two matrix whether equal, A = A
/// @param: <m1> the first matrix [ const Matrix * ]
/// @param: <m2> the second matrix [ const Matrix * ]
/// @return: 1 for true, 0 for false [ _Bool ]
_Bool IsEqualMatrix(const Matrix *m1, const Matrix *m2) {
  if (m1->size[0] != m2->size[0] || m1->size[1] != m2->size[1]) {
    return 0;
  }

  for (size_t i = 1; i <= m1->size[0]; ++i) {
    for (size_t j = 1; j <= m2->size[1]; ++j) {
      if (GetMatrixValue(m1, i, j) != GetMatrixValue(m2, i, j)) {
        return 0;
      }
    }
  }

  return 1;
}

/// @func: IsSymmetricMatrix
/// >> test a matrix whether a symmetric matrix, A^T = A
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: 1 for true, 0 for false [ _Bool ]
_Bool IsSymmetricMatrix(const Matrix *mat) {
  Matrix *trans = TransposeMatrix(mat);
  _Bool flag = IsEqualMatrix(trans, mat);
  DestroyMatrix(trans);
  return flag;
}

/// @func: IsAntisymmetricMatrix
/// >> test a matrix whether a antisymmetric matrix, A^T = -A
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: 1 for true, 0 for false [ _Bool ]
_Bool IsAntisymmetricMatrix(const Matrix *mat) {
  Matrix *trans = TransposeMatrix(mat);
  Matrix *neg = ScalarMatrixMul(-1, mat);
  _Bool flag = IsEqualMatrix(trans, neg);
  DestroyMatrix(neg);
  DestroyMatrix(trans);
  return flag;
}

/// @func: IsNormalMatrix
/// >> test a matrix whether a normal matrix, A^T A = A A^T
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: 1 for true, 0 for false [ _Bool ]
_Bool IsNormalMatrix(const Matrix *mat) {
  Matrix *trans = TransposeMatrix(mat);
  Matrix *ata = MatrixMul(trans, mat);
  Matrix *aat = MatrixMul(mat, trans);
  _Bool flag = IsEqualMatrix(ata, aat);
  DestroyMatrix(aat);
  DestroyMatrix(ata);
  DestroyMatrix(trans);
  return flag;
}

/// @func: IsOrthogonalMatrix
/// >> test a matrix whether a orthogonal matrix, A^T = A^(-1)
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: 1 for true, 0 for false [ _Bool ]
_Bool IsOrthogonalMatrix(const Matrix *mat) {
  Matrix *inv = InverseMatrix(mat);
  Matrix *trans = TransposeMatrix(mat);
  _Bool flag = IsEqualMatrix(inv, trans);
  DestroyMatrix(trans);
  DestroyMatrix(inv);
  return flag;
}

/// @func: MatrixDeterminant
/// >> get the determinant of matrix
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: the determinant [ double ]
/// @descript:
///   * exit when `mat` is a null pointer
///   * exit when `mat` is not a square matrix
double MatrixDeterminant(const Matrix *mat) {
  if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (mat->size[0] != mat->size[1]) {
    fprintf(stderr, "Not a Square Matrix Error!");
    exit(EXIT_FAILURE);
  }

  LURes *lures = MatrixLUDecomposition(mat);
  double det = 1.0;

  for (size_t i = 1; i <= lures->matrixU->size[0]; ++i) {
    det *= GetMatrixValue(lures->matrixU, i, i);
  }
  det *= lures->factor;

  DestroyLURes(lures);

  return det;
}

/// @func: InverseMatrix
/// >> get the inverse of matrix
/// @param: <mat> the matrix [ const Matrix * ]
/// @return: the inverse matrix [ Matrix * ]
/// @descript:
///   * exit when `mat` is not a square matrix;
///   * exit when `mat` is a null pointer
///   * exit when `mat` is not inversible
Matrix *InverseMatrix(const Matrix *mat) {
  if (NULL == mat) {
    fprintf(stderr, "Null Pointer Error!");
    exit(EXIT_FAILURE);
  } else if (mat->size[0] != mat->size[1]) {
    fprintf(stderr, "Not a Square Matrix Error!");
    exit(EXIT_FAILURE);
  }

  double detM = MatrixDeterminant(mat);
#define abs(x) ((x) < 0 ? -(x) : (x))
  const double tol = 1e-5;
  if (abs(detM) < tol) {
    fprintf(stderr, "Not Invertible Error!");
    exit(EXIT_FAILURE);
  }
#undef abs

  return ScalarMatrixDiv(InitAdjugateMatrix(mat), detM);
}
